% function K  = LQR_AB_K(L,R)
% function  LQR_AB_K(L,R)

%  K=LQR_AB_K(0.18,0.18)
%定义机器人机体参数
syms R_w                       % 驱动轮半径
syms R_l                       % 驱动轮轮距/2
syms L_l  L_r                  % 左右腿长
syms L_wl L_wr                 % 驱动轮质心到左右腿部质心距离
syms L_bl L_br                 % 机体质心到左右腿部质心距离
syms L_c                       % 机体质心到腿部关节中心点距离
syms M_w M_l M_b               % 驱动轮质量 腿部质量 机体质量
syms I_w                       % 驱动轮转动惯量                (自然坐标系法向)
syms I_ll I_lr                 % 驱动轮左右腿部转动惯量        (自然坐标系法向，实际上会变化)
syms I_b                       % 机体转动惯量                  (自然坐标系法向)
syms I_z                       % 机器人z轴转动惯量             (简化为常量)

% 定义其他独立变量并补充其导数
syms theta_wl    theta_wr      %左右驱动轮转角
syms dtheta_wl   dtheta_wr 
syms ddtheta_wl  ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b

% 定义状态向量
syms   s
syms   ds 
syms   phi dphi 
syms   theta_ll dtheta_ll 
syms   theta_lr dtheta_lr 
syms   theta_b  dtheta_b

% 定义控制向量
syms  T_wl T_wr
syms  T_bl T_br

%定义LQR输入输出量
syms X Xdot Xddot %输入
syms U            %输出

% 输入物理参数：重力加速度
syms g

g = 9.81;

R_w=0.062;     %驱动轮半径                   （单位：m）    
I_w=0.001;     %驱动轮转动惯量               （单位：kg/m^2）
M_w=0.648;     %驱动轮质量 0.283             （单位：kg）
R_l=0.44/2;    %驱动轮轮距/2                 （单位：m）

I_b=0.208;       %机体转动惯量Px               （单位：kg/m^2）
L_c=0.15315;     %机体质心到腿部关节中心点距离 （单位： m）
M_b=12.494;      %机体质量                     （单位：kg）
I_z=0.613;       %机器人z轴转动惯量              (单位：kg/m^2）

M_l=2.054;       %腿部质量                  （单位：kg）

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%：                      变量参数                       ：%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 左腿长                                    （单位：m）
L_l=0.18;                                      
%机体到左腿部质心的距离                     （单位：m）           
L_bl = L_l - ( -1.3153 * L_l^3 + 1.6936  * L_l^2  + -0.1229 * L_l    + 0.2224);
%驱动轮质心到左腿部质心距离                （单位：m）     
L_wl =         -1.2603* L_l^3 +  1.3962  * L_l^2  +  -0.2799  * L_l  + 0.0803;
%驱动轮左腿部转动惯量                       （单位：kg/m^2）
%  I_ll =         -3.0771*L_l^4 +   3.3794 * L_l^3  +   -1.4337 * L_l^2  +0.2749 * L_l + 0.0318; 
 I_ll =M_b*((L_l)^2+0.05^2)/12.0;




% I_ll =M_w*R_w^2;                      %驱动轮转动惯量
% Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0;    %摆杆转动惯量
% IM1=M1*(0.3^2+0.12^2)/12.0;          %机体绕质心转动惯量



% 右腿长
L_r =L_l;
%机体到右腿部质心的距离
L_br = L_bl;
% 驱动轮质心到右腿部质心距离
L_wr =L_wl;
%驱动轮右腿部转动惯量
I_lr = I_ll; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%：                      变量参数                       ：%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%

%% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%：                   L0=0.18时参数                      ：%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% 
%  %左腿长
% L_l  = 0.18;
% %机体到左腿部质心的距离
% L_bl = 0.204 ;
% %驱动轮质心到左腿部质心距离
% L_wl = 0.08309;
% %驱动轮左腿部转动惯量
% I_ll = 0.024535; 
% 
% % 右腿长
% L_r = 0.18;
% %机体到右腿部质心的距离
% L_br = L_bl;
% % 驱动轮质心到右腿部质心距离
% L_wr = L_wl;
% %驱动轮右腿部转动惯量
% I_lr = I_ll; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%：                      变量参数                       ：%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%

% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
A = zeros(10,10);
B = zeros(10,4);
% A = sym('A',[10 10]);
% B = sym('B',[10 4]);
%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%

% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
eqn1 =  (I_w * L_l / R_w + M_w * R_w * L_l + M_l * R_w * L_bl) * ddtheta_wl   + (M_l * L_wl * L_bl-I_ll) * ddtheta_ll + (M_l * L_wl  + M_b * L_l / 2) * g * theta_ll + T_bl - T_wl * (1 + L_l / R_w)==0;
eqn2 =  (I_w * L_r / R_w + M_w * R_w * L_r + M_l * R_w * L_br) * ddtheta_wr   + (M_l * L_wr * L_br-I_lr) * ddtheta_lr + (M_l * L_wr  + M_b * L_r / 2) * g * theta_lr + T_br - T_wr * (1 + L_r / R_w)==0;
eqn3 = -(M_w * R_w * R_w + I_w + M_l * R_w * R_w + M_b * R_w   * R_w / 2 )    *  ddtheta_wl - (M_w * R_w * R_w+I_w + M_l * R_w * R_w + M_b * R_w * R_w / 2) * ddtheta_wr-(M_l * R_w * L_wl + M_b * R_w * L_l / 2) * ddtheta_ll - (M_l * R_w * L_wr + M_b * R_w * L_r / 2 ) * ddtheta_lr + T_wl + T_wr==0;
eqn4 =  (M_w * R_w * L_c + I_w * L_c / R_w + M_l * R_w * L_c )  * ddtheta_wl   + (M_w * R_w * L_c + I_w * L_c / R_w + M_l * R_w * L_c ) * ddtheta_wr + M_l * L_wl * L_c * ddtheta_ll + M_l * L_wr * L_c * ddtheta_lr - I_b * ddtheta_b + M_b * g * L_c * theta_b - (T_wl + T_wr) * L_c / R_w - (T_bl + T_br)==0;
eqn5 =  ((I_z * R_w)/ (2 * R_l)+ I_w * R_l / R_w) * ddtheta_wl - ((I_z * R_w) / (2 * R_l) + I_w * R_l / R_w) * ddtheta_wr + (I_z * L_l) / (2 * R_l) * ddtheta_ll - (I_z * L_r) / (2 * R_l) * ddtheta_lr - T_wl * R_l / R_w + T_wr * R_l / R_w==0;
[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);

% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);


% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
for p = 5:2:9
    A_index = (p - 3)/2;
    A(2,p) =  R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
    A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (L_l*J_A(3,A_index))/(2*R_l) + (L_r*J_A(4,A_index))/(2*R_l);
    for q = 6:2:10
        A(q,p) = J_A(q/2,A_index);
    end
end

% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
for r = 1:10
    if rem(r,2) == 0
        A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
    else
        A(r,:) = zeros(1,10);
        A(r,r+1) = 1;
    end
end

% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
for h = 1:4
    B(2,h) =  R_w*(J_B(1,h) + J_B(2,h))/2;
    B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (L_l*J_B(3,h))/(2*R_l) + (L_r*J_B(4,h))/(2*R_l);
    for f = 6:2:10
        B(f,h) = J_B(f/2,h);
    end
end

% B的其余数值为0
for e = 1:2:9
    B(e,:) = zeros(1,4);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                           此处可以输入QR矩阵                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 矩阵Q中，以下列分别对应：
%              s    ds     phi    dphi   theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
lqr_Q = diag([1, 5,    100,   1000,     3000,        1,       3000,     1,     8000,    10]);
% 其中：
% s       : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
% phi     ：机器人水平方向移动时yaw偏航角度，dphi为其导数
% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数

% 矩阵中，以下列分别对应：
%            T_wl    T_wr    T_bl   T_br
lqr_R=diag([  50.00, 50.00, 1, 1]);


% 其中：
% T_wl: 左侧驱动轮输出力矩
% T_wr：右侧驱动轮输出力矩
% T_bl：左侧髋关节输出力矩
% T_br：右腿髋关节输出力矩
% 单位皆为Nm

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

K=lqr(A,B,lqr_Q,lqr_R);

% 获取矩阵的大小
[m, n] = size(K);

% 输出矩阵，元素之间用逗号分隔
for i = 1:m
    for j = 1:n
        % 输出每个元素，如果是正数则加上正号
        if K(i, j) >= 0
            fprintf('+%.4f', K(i, j)); % 输出正数时加上正号
        else
            fprintf('%.4f', K(i, j)); % 输出负数
        end
        if j < n
            fprintf(','); % 如果不是行的最后一个元素，输出逗号
        end
    end
    fprintf(','); fprintf('\n'); % 换行
end

% end







